/*
 * transport.c 
 *
 * CSC458 A2 (Reliable Transport)
 *
 * This file implements the STCP layer that sits between the
 * mysocket and network layers. You are required to fill in the STCP
 * functionality in this file. 
 *
 */

#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include <arpa/inet.h>
#include "mysock.h"
#include "stcp_api.h"
#include "transport.h"
#include <sys/types.h>
#include <unistd.h>
#include <sys/time.h>
#include <time.h>
#include <pthread.h>


/******************************* CONSTANTS *****************************/

#define RECIEVER_WIN_SIZE       3072    // Reciever window size
#define CONGESTION_WIN_SIZE     3072    // Congestion window size
#define MAX_INIT_SEQ_NUM        255     // Maximum initial sequence number
#define TIMEOUT_SLEEP           300     // When retransmit thread will run
#define MAX_TIMEOUT_SEC         1       // Default timeout time of 300 milliseconds
#define MAX_RETRANSMIT_ATTEMPTS 5       // Default number of retransmissions

// TCP_DATA_START does not work as expected.
#define TCP_START_DATA(p) (((char *) p) + (((STCPHeader *) p)->th_off * sizeof(uint32_t)))

/***********************************************************************/

// Connection states from RFC793
enum {   LISTEN, SYN_SENT, SYN_RECEIVED, ESTABLISHED,
         FIN_WAIT_1, FIN_WAIT_2, CLOSE_WAIT, CLOSING,
         LAST_ACK, TIME_WAIT, CLOSED};
// TODO: Insert the 3 groups of states


/* Structure representing segments placed on the retranmission and
 * out_of_order queues 
 */
struct segment
{
    struct segment *next;
    uint16_t data_len;
    STCPHeader *packet;
    struct timeval *timeout;
    int num_retransmission;
};

/* Structure representing the send sequence space */
typedef struct
{
   tcp_seq snd_una;  // Sequence number of first byte of data that has been
                     // sent but unacknowledged.
   tcp_seq snd_nxt;  // Send next (next sequence number to be sent)
   uint16_t snd_wnd; // Send window (size = min(|rcv.wnd|,
                     //                         |CONGESTION_WIN_SIZE|))
} send_seq_space;

/* Structure representing the recieve sequence spcace */
typedef struct
{
   tcp_seq rcv_nxt;  // Receive next (next sequence number expected)
   tcp_seq rcv_wnd;  // Recieve window size advertised to the sender 
} recieve_seq_space;

/* this structure is global to a mysocket descriptor */
typedef struct
{
    bool_t done;                    /* TRUE once connection is closed */

    int connection_state;           /* state of the connection */
    tcp_seq initial_sequence_num;

    /* any other connection-wide global variables go here
     * (reprsents the TCB) */
    tcp_seq last_ack;
    // Retransmission queue, in order that sent out.
    struct segment *retransmit;

    // Lock for retransmission queue.
    pthread_mutex_t retransmit_mutex;

    // Out of order segment queue, in order by sequence number.
    struct segment *out_of_order;


    send_seq_space send_space;
    recieve_seq_space receive_space;
} context_t;

/* Structure holding arguments needed by the retransmission timer thread */
typedef struct
{
    context_t *ctx;
    mysocket_t sd;
} timer_data_t;

static void generate_initial_seq_num(context_t *ctx);
static void control_loop(mysocket_t sd, context_t *ctx);

/******************* RETRANSMISSION TIMER HELPER FUNCTIONS *******************/

// Note: These should be in they're own file (in a perfect world)

static void start_retransmission_timer(mysocket_t sd, context_t *ctx);


/*****************************************************************************/


/*********************** MY TRANSPORT HELPER FUNCTIONS ***********************/
// Create a TCP packet. If flags, th_seq, th_ack, and len are not
// specified use 0 as the default value.
static STCPHeader *create_tcp_packet(uint8_t th_flags = 0, tcp_seq th_seq = 0,
                                    tcp_seq th_ack = 0, int len = 0,
                                    STCPHeader *packet = NULL);
// Create and send a FIN packet.
static int create_send_fin(mysocket_t sd, context_t *ctx);
// Create and send an ACK packet.
static void create_send_ack(mysocket_t sd, context_t *ctx, STCPHeader *packet);

// Helpers to check if receieved packets are the ones we expected and want.
static bool_t is_in_order(context_t *ctx, STCPHeader *header);
static bool_t is_valid_ack(context_t *ctx, STCPHeader *header);
static bool_t check_out_of_order(mysocket_t sd, context_t *ctx);

// Print helper functions.
static void print_context(context_t *ctx);
static void print_data(char *buf, int len);
static void print_queue(struct segment *queue);
static void print_state(context_t *ctx);
static void print_out_of_order(context_t *ctx);

// Update the send and receieve contexts.
static void update_context_send(context_t *ctx, int len);
static void update_context_receive(context_t *ctx, bool is_ack);
static void update_send_wnd(context_t *ctx, uint16_t wnd);

// Get the current usable window.
static int get_usuable_window_size(context_t *ctx);

// Check if the given packet is in the current window.
static bool_t is_in_window(context_t *ctx, STCPHeader *header, int data_len);

// Return a timeout for a sent packet.
static struct timeval *get_timeout();

// Helper methods to send data to the peer.
static bool_t check_can_send(context_t *ctx);
static void send_data(mysocket_t sd, context_t *ctx, char *buf, int len);
static int app_send(mysocket_t sd, context_t *ctx);

// Send a packet and enqueue it on the retransmission queue.
static int create_send_enqueue_packets(mysocket_t sd, context_t *ctx, char *buf,
                                int len);
static void enqueue_retransmit_segment(context_t *ctx, STCPHeader *packet,
                                        int len);

// Check connection state.
static bool_t check_connection_closed(context_t *ctx);
static bool_t check_connection_established(context_t *ctx);
static bool_t check_connection_listen(context_t *ctx);

// Process a received packet.
static void network_receive(mysocket_t sd, context_t *ctx, char *packet, int len);
static bool_t process_receive(mysocket_t sd, context_t *ctx, STCPHeader *header, 
                        int len);

// Malloc and fill a segment helper functions.
static struct segment *create_segment(struct segment *next, int data_len,
                                     STCPHeader *packet,
                                     struct timespec *timeout, int num_rt);
static void segment_fill_packet(struct segment *segment, STCPHeader *packet,
                                int len);

static struct segment *dequeue_segment(tcp_seq th_ack, struct segment *queue, bool_t *removed);
static struct segment *insert_out_of_order_segment(context_t *ctx,
                                        STCPHeader *packet, int len);

static struct segment *get_tail(struct segment *queue);
static void free_segment (struct segment *segment);


// Process a close request from the application.
static int process_close(mysocket_t sd, context_t *ctx);

static void *retransmit_timer(void *threadarg);


/***********************************************************************/

/* initialise the transport layer, and start the main loop, handling
 * any data from the peer or the application.  this function should not
 * return until the connection is closed.
 */
// TODO: Break function into helper methods.
void transport_init(mysocket_t sd, bool_t is_active)
{
    context_t *ctx;
    ssize_t bytes_recv;
    unsigned int rc;
    char buf[sizeof(STCPHeader) + STCP_MSS];
    STCPHeader *syn_header;
    //struct timeval *timeout;

    ctx = (context_t *) calloc(1, sizeof(context_t));
    assert(ctx);

    generate_initial_seq_num(ctx);

    /* XXX: you should send a SYN packet here if is_active, or wait for one
     * to arrive if !is_active.  after the handshake completes, unblock the
     * application with stcp_unblock_application(sd).  you may also use
     * this to communicate an error condition back to the application, e.g.
     * if connection fails; to do so, just set errno appropriately (e.g. to
     * ECONNREFUSED, etc.) before calling the function.
     */
    ctx->connection_state = CLOSED;

    // Setup context send variables.
    ctx->send_space.snd_una = ctx->initial_sequence_num;
    ctx->send_space.snd_nxt = ctx->initial_sequence_num;

    /* If the connection should be initiated so perform the three-way SYN
     * handshake.
     *
     * Perform the following three steps: 
     * 1.   The requesting active end(us) sends a SYN packet to the other end
     *      with the appropriate seq number (seqx) in the header. The active
     *      end then sits waiting for a SYN_ACK.
     * 2.   The passive end sends a SYN_ACK with its own sequence number (seqy)
     *      and acks with the number (seqx+1). The passive end waits for an ACK.
     * 3.   The active end records seqy so that it will expect the next byte
     *      from the passive end to  start at (seqy+1). It ACKs the passive end's
     *      SYN request, and changes its state to the established state.
     *
     *      TODO: Implement connection using retransmission timer
     */
    if(is_active) // SENDING TCP
    {

        // Create and send the SYN packet.
        syn_header = create_tcp_packet(TH_SYN,ctx->initial_sequence_num);
        bytes_recv = stcp_network_send(sd, syn_header, sizeof(STCPHeader), NULL);

        // Send failed.
        if (bytes_recv <= 0)
        {
            printf("* Sending SYN failed: Connection refused\n");
            errno = ECONNREFUSED;
        }
        else
        {
            // Set connection state.
            ctx->connection_state = SYN_SENT;
            // Increment snd_nxt.
            update_context_send(ctx, 1);
        }

        // While we have not recieved an event that changed the connection state.
        while (ctx->connection_state == SYN_SENT)
        {
            //timeout = get_timeout();
            rc = stcp_wait_for_event(sd, ANY_EVENT, NULL);

            if (rc & NETWORK_DATA)
            {
               // Get the SYN or ACK packet from receiver.
               bytes_recv = stcp_network_recv(sd, buf, sizeof(buf));
               syn_header = (STCPHeader *)buf;

               if ((unsigned) bytes_recv < sizeof(STCPHeader) || 
                   syn_header->th_flags != (TH_SYN | TH_ACK))
               {
                  // TODO: Bad
                  printf("* Received wrong packet: expecting SYN or ACK\n");
                  continue;
               }
               else
               {
                  // Make sure the acknowledge number is the one we were expecting.
                  if (ntohl(syn_header->th_ack) != (ctx->send_space.snd_una) + 1)
                  {
                     printf("* Recieved INVALID ACK number: %d Expected: %d\n", 
                              ntohl(syn_header->th_ack), ctx->send_space.snd_una);
                     continue;
                  }
                  else
                  {
                     ctx->connection_state = ESTABLISHED;
                     update_send_wnd(ctx, ntohs(syn_header->th_win));
                     ctx->receive_space.rcv_wnd =  ntohs(syn_header->th_win);

                     if (syn_header->th_flags & TH_SYN)
                     {
                        // Set initial rcv_nxt.
                        ctx->receive_space.rcv_nxt = ntohl(syn_header->th_seq);
                     }

                     update_context_receive(ctx, (syn_header->th_flags & TH_ACK));

                    // Send acknowledgment.
                    create_send_ack(sd, ctx, syn_header);
                    // Since we have not yet sent data, set snd.una to snd.nxt
                    // (this only happens on initiation)
                    ctx->send_space.snd_una = ctx->send_space.snd_nxt;
                    ctx->last_ack = ctx->send_space.snd_nxt - 1;
                  }
               }
            }
            else if (rc & APP_CLOSE_REQUESTED)
            {
            }
            else if (rc & TIMEOUT)
            {
               // TODO: Retransmit until it has been sent 6 times
            }
        }
    }
    else // RECEIVING TCP
    {    // While the connection isn't open.
         while (ctx->connection_state == CLOSED)
         {
            //timeout = get_timeout();
            rc = stcp_wait_for_event(sd, NETWORK_DATA, NULL);

            if (rc & APP_DATA)
            {
            }
            else if (rc & NETWORK_DATA)
            {
                bytes_recv = stcp_network_recv(sd, buf, sizeof(buf));
                syn_header = (STCPHeader *)buf;

                // 
                if ((unsigned) bytes_recv < sizeof(STCPHeader) || 
                       syn_header->th_flags != TH_SYN)
                {
                    // TODO: Bad
                    printf("* Received wrong packet: expecting SYN ACK\n");
                    continue;
                }
                else
                {
                  // Change the connection state.
                  ctx->connection_state = SYN_RECEIVED;
                  // Set send window.
                  ctx->send_space.snd_wnd = ntohs(syn_header->th_win);
                  // Set initial rcv_nxt.
                  ctx->receive_space.rcv_nxt = ntohl(syn_header->th_seq); 

                  update_context_receive(ctx, (syn_header->th_flags & TH_ACK));

                  // Send SYN and acknowledgement.
                  syn_header = create_tcp_packet(TH_SYN | TH_ACK,
                                 ctx->initial_sequence_num, ctx->receive_space.rcv_nxt);
                  bytes_recv = stcp_network_send(sd, syn_header, sizeof(STCPHeader), NULL);

                  // If send failed.
                  if (bytes_recv < 0)
                  {
                     printf("* Sending ACK failed: connected aborted\n");
                     errno = ECONNABORTED;
                     break;
                  }
                  else
                  {
                     update_context_send(ctx, 1);

                     // Wait for the ACK packet.
                     while (ctx->connection_state == SYN_RECEIVED)
                     {
                        //timeout = get_timeout();
                        rc = stcp_wait_for_event(sd, ANY_EVENT, NULL);

                        if (rc & NETWORK_DATA)
                        {
                           // Get the ACK packet
                           bytes_recv = stcp_network_recv(sd, buf, sizeof(buf));
                           syn_header = (STCPHeader *)buf;

                           if ((unsigned) bytes_recv < sizeof(STCPHeader) || 
                                syn_header->th_flags != TH_ACK || 
                                  ctx->receive_space.rcv_nxt != ntohl(syn_header->th_seq))
                           {
                              // TODO: Bad
                              printf("* Received wrong packet: expecting ACK\n");
                              continue;
                           }
                           else
                           {
                              update_context_receive(ctx, 1);
                              // Since no data has been sent, snd_una = snd_nxt
                              ctx->send_space.snd_una = ctx->send_space.snd_nxt;
                              update_send_wnd(ctx, ntohs(syn_header->th_win));
                              ctx->receive_space.rcv_wnd =  ntohs(syn_header->th_win);
                              ctx->connection_state = ESTABLISHED;
                              ctx->last_ack = ctx->send_space.snd_nxt - 1;
                           }
                        }
                     }
                  }
               }
                }
            else if (rc & APP_CLOSE_REQUESTED)
            {

            }
            else if (rc & TIMEOUT)
            {
               // TODO: Retransmit until it has been sent 6 times.
            }
         }
    }

    // Start the retransmission timeout thread.

    // TODO: Move to helper method.
    timer_data_t *timer_data = (timer_data_t *)malloc(sizeof(timer_data_t));

    assert(timer_data);

    timer_data->sd = sd;
    timer_data->ctx = ctx;

    pthread_mutex_init((&ctx->retransmit_mutex), NULL);
    pthread_t timer_thread;
    int return_code;
    //int *status;

    if ((return_code = pthread_create(&timer_thread, NULL, retransmit_timer,
                                        timer_data))) {
        printf("ERROR; return code from pthread_create() is %d\n", return_code);
        exit(-1);
    }

    stcp_unblock_application(sd);
    control_loop(sd, ctx);

    // Join with the retransmission timeout thread and wait for it to exit.
    if ((return_code = pthread_join(timer_thread, NULL)))
    {
        printf("ERROR; return code from pthread_join() is %d\n", return_code);
        exit(-1);
    }

    pthread_mutex_destroy((&ctx->retransmit_mutex));
    free(timer_data);

    /* do any cleanup here */
    //free(timeout);
    free(ctx->retransmit);
    free(ctx->out_of_order);
    free(ctx);

    return;
}

/* retransmit_timer
 *
 * Called every MAX_TIMEOUT_MSEC, loops over the retransmission queue and
 * performs the following steps:
 *      1.  If a segment is over it's timeout time
 *          (i.e., timeout > current time), resend it and increase the
 *          num_retransmission count.
 *      2.  If a segment is over the amount of retransmissions
 *          (i.e., num_retransmission > MAX_RETRANSMIT_ATTEMPTS), the network
 *          is assumed to have failed
 */
static void *retransmit_timer(void *threadarg)
{
    timer_data_t *timer_data = (timer_data_t *)threadarg;
    assert(timer_data);

    struct segment *head;
    struct timeval current;
    ssize_t bytes_sent;

    while (!timer_data->ctx->done)
    {
        // Acquire lock.
        pthread_mutex_lock(&(timer_data->ctx->retransmit_mutex));

        // Loop over the retransmit queue.
        head = timer_data->ctx->retransmit;

        while(head != NULL)
        {
            gettimeofday(&current, NULL);

            // If this segment has reached it's timeout.
            if (timercmp(head->timeout, &current, <))
            {
                // If it's reached it's max number of retransmissions, set errno
                // and ctx->done and return.
                if (head->num_retransmission >= MAX_RETRANSMIT_ATTEMPTS)
                {
                    printf("* ERROR reached max # of retransmissions, network failed\n");
                    errno = ECONNABORTED;
                    timer_data->ctx->done = TRUE;
                }
                // Else, retransmit the segment.
                else
                {
                    // TODO: Move this to a helper method.
                    bytes_sent = stcp_network_send(timer_data->sd, head->packet,
                                    (sizeof(STCPHeader) + head->data_len), NULL);

                    // Send failed.
                    if (bytes_sent <= 0)
                    {
                        printf("* Sending retransmission failed: Connection refused\n");
                        errno = ECONNREFUSED;
                        timer_data->ctx->done = TRUE;
                    }
                    else
                    {
                        head->num_retransmission++;
                        head->timeout = get_timeout();
                    }
                }
               //printf("**********************************************************************\n");
            }

            head = head->next;
        }

        // Release lock.
        pthread_mutex_unlock(&(timer_data->ctx->retransmit_mutex));

        usleep(TIMEOUT_SLEEP);
    }

    pthread_exit(NULL);
}

/* generate random initial sequence number for an STCP connection */
static void generate_initial_seq_num(context_t *ctx)
{
    assert(ctx);

#ifdef FIXED_INITNUM
    /* please don't change this! */
    ctx->initial_sequence_num = 1;
#else
    /* you have to fill this up */

    // Generate random sequence number between 0 and 255, both inclusive.
    // Use getpid() to randomize the number, otherwise, the initial sequence
    // number for the client and server is always the same.
    srand((unsigned int)time(NULL) + getpid());
    ctx->initial_sequence_num = rand() % (MAX_INIT_SEQ_NUM + 1);
#endif
}


/* control_loop() is the main STCP loop; it repeatedly waits for one of the
 * following to happen:
 *   - incoming data from the peer
 *   - new data from the application (via mywrite())
 *   - the socket to be closed (via myclose())
 *   - a timeout
 */
static void control_loop(mysocket_t sd, context_t *ctx)
{
    assert(ctx);
    ssize_t len;
    unsigned int event;
    // TODO: Fix this size.
    char packet[sizeof(STCPHeader) + 40 + STCP_MSS];

    while (!ctx->done)
    {
        /* see stcp_api.h or stcp_api.c for details of this function */
        /* XXX: you will need to change some of these arguments! */
        event = stcp_wait_for_event(sd, ANY_EVENT, NULL);

        // SEND CALL: The application requested data to be sent to the server
        if (event & APP_DATA)
        {
            // Check that we are allowed to send data.
            if (check_can_send(ctx))
            {
                // Possibly send the data to the server.
                if ((app_send(sd, ctx)) < 0)
                {
                    // An error happened while sending, which we can't recover
                    // from. So return.
                    return;
                }
            }
        }

        // Segment from a peer.
        if (event & NETWORK_DATA)
        {
            len = stcp_network_recv(sd, packet, sizeof(packet));

            // Make sure we received a TCP packet, otherwise igonore it.
            //if (len > 0)
            if ((unsigned) len >= sizeof(STCPHeader))
            {
                if (!check_connection_closed(ctx) && !check_connection_listen(ctx))
                {
                    network_receive(sd, ctx, packet, len);
                }
            }
        }

        // User requests to close the connection.
        if (event & APP_CLOSE_REQUESTED)
        {
            if ((process_close(sd, ctx)) < 0)
            {
                // An error happened while sending, which we can't recover
                // from. So return.
                return;
            }
        }

        // Timeout.
        if (event & TIMEOUT)
        {
            // TODO:
        }
    }

}

/* check_can_send
 *
 * Return whether the connection is in a state that new data can be sent.
 */
static bool_t check_can_send(context_t *ctx)
{
    return  ctx->connection_state != CLOSED && 
            ctx->connection_state != FIN_WAIT_1 &&
            ctx->connection_state != FIN_WAIT_2 &&
            ctx->connection_state != CLOSE_WAIT &&
            ctx->connection_state != CLOSING &&
            ctx->connection_state != LAST_ACK;
}

/* check_connection_closed
 *
 * Returns whether the connection is closed. Sets errno if it is.
 */
static bool_t check_connection_closed(context_t *ctx)
{
    // Sanity check.
    assert(ctx);

    bool_t is_closed = ctx->connection_state == CLOSED;
    // If the connection is not open, return an error.
    if (is_closed)
    {
        printf("* ERROR cannot send data when connection closed aborted\n");
        errno = ECONNABORTED;
    }

    return is_closed;
}

/* check_connection_established
 *
 * Returns whether the connection is established.
 */
static bool_t check_connection_established(context_t *ctx)
{
    return ctx->connection_state == ESTABLISHED;
}

/* check_connection_listen
 *
 * Returns whether the connection is in a listening state.
 */
static bool_t check_connection_listen(context_t *ctx)
{
    return ctx->connection_state == LISTEN;
}

/* app_send
 *
 * Retrieve as much data from the application that we are able to send and
 * send it to the server. Calls create_send_enqueue_packets which handles
 * creating the STCP packets, sending them to the server and enqueuing them
 * on the retransmission queue.
 * 
 * Return 0 if the send was successful or we must wait for an ACK, -1 if it
 * failed.
 */
static int app_send(mysocket_t sd, context_t *ctx)
{
    ssize_t len;
    int succ = 0;

    /* Check how much data we can send. */

    // The size of the usuable window.
    int usable_wind = get_usuable_window_size(ctx);

    // If we can send data.
    if (usable_wind > 0)
    {
        // Read the sendable data from the application.
        char *buf = (char *)calloc(1, usable_wind);
        assert(buf);

        len = stcp_app_recv(sd, buf, usable_wind);

        // If we have data to send to the server.
        if (len > 0)
        {
            // Break data into segment(s) and place on unsent queue.
            if ((create_send_enqueue_packets(sd, ctx, buf, len)) <= 0)
            {
                // If the send failed, we can't recover from this.
                succ = -1;
            }
        }

        free(buf);
    }

    // Otherwise, we have to wait for an ACK. So return.

    return succ;
}

/* get_usuable_window_size
 *
 * Return the current usable window size.
 */
static int get_usuable_window_size(context_t *ctx)
{
    // Sanity check
    assert(ctx);

    // The size of the usuable window: (snd.una + snd.wnd) - snd.nxt
    return (ctx->send_space.snd_una + ctx->send_space.snd_wnd) -
                      ctx->send_space.snd_nxt;
}

/* create_enqueue_packets
 *
 * Creates TCP packets to be sent to the server. If the size of the data is
 * greater than the maximum payload size, it is split between packets.
 * The packets are then sent and placed on the retransmit queue.
 *
 * Returns the number of bytes sent.
 *
 * Note: the calling function must free buf.
 */
static int create_send_enqueue_packets(mysocket_t sd, context_t *ctx, char *buf, int len)
{
    // Sanity checks.
    assert(ctx);
    assert(buf);

    STCPHeader *packet;
    ssize_t bytes_sent;

    // Check if the packets need to be split.
    while (len > 0)
    {
        int cur_len = MIN(len, STCP_MSS);

        // Create the packet to be sent. This must be freed after it
        // it dequeued from the retransmission queue! (By calling
        // dequeue_segment).
        packet =
            create_tcp_packet(0, ctx->send_space.snd_nxt,
                              ctx->receive_space.rcv_nxt,cur_len);

        /* Send the packet */

        // Copy the data into the packet.
        memcpy((char *) TCP_START_DATA(packet), buf, cur_len);

        bytes_sent =
            stcp_network_send(sd, packet, (sizeof(STCPHeader) + cur_len), NULL);

        // Send failed.
        if (bytes_sent <= 0)
        {
            printf("* Sending data failed: Connection refused\n");
            errno = ECONNREFUSED;
            break;
        }
        else
        {
            // Increment snd_nxt.
            update_context_send(ctx, cur_len);

            // Place the packet on the retransmission queue.
            enqueue_retransmit_segment(ctx, packet, cur_len);
        }

        len = len - cur_len;
    }

    return bytes_sent;
}


/* network_receive
 *
 * Implement the TCP state machine.
 */
static void network_receive(mysocket_t sd, context_t *ctx, char *packet, int len)
{
    // Sanity checks.
    assert(ctx);
    assert(packet);

    STCPHeader *header = (STCPHeader *)packet;

    // Make sure we know about the advertised receive window.
    update_send_wnd(ctx, ntohs(header->th_win));

    // Process the packet depending on the current state.
    switch (ctx->connection_state)
    {
    case ESTABLISHED:
        if (process_receive(sd, ctx, header, len))
        {
            // If we recieved a FIN.
            if (header->th_flags & TH_FIN)
            {
                ctx->connection_state = CLOSE_WAIT;
            }
        }
        else
        {
            // Something bad happened.
            // TODO: something.
        }
        break;

    case FIN_WAIT_1:
        if (process_receive(sd, ctx, header, len))
        {
            // If we recieved an ACK.
            if (header->th_flags & TH_ACK)
            {
                ctx->connection_state = FIN_WAIT_2;
            }

            // If we recieved a FIN
            if (header->th_flags & TH_FIN)
            {
                ctx->connection_state = CLOSING;
            }
        }
        else
        {
            // Something bad happened.
            // TODO: something.
        }
        break;

    case FIN_WAIT_2:
        if (process_receive(sd, ctx, header, len))
        {
            // If we recieved a FIN
            if (header->th_flags & TH_FIN)
            {
                ctx->connection_state = CLOSED;
                ctx->done = TRUE;
                return;
            }
        }
        else
        {
            // Something bad happened.
            // TODO: something.
        }
        break;

    case CLOSING:
        if (process_receive(sd, ctx, header, len))
        {
            // If we recieved an ACK.
            if (header->th_flags & TH_ACK)
            {
                ctx->connection_state = CLOSED;
                ctx->done = TRUE;
                return;
            }
        }
        else
        {
            // Something bad happened.
            // TODO: something.
        }
        break;

    case CLOSE_WAIT:
        break;

    case LAST_ACK:
        if (process_receive(sd, ctx, header, len))
        {
            // If we recieved an ACK.
            if (header->th_flags & TH_ACK)
            {
                ctx->connection_state = CLOSED;
                ctx->done = TRUE;
                return;
            }
        }
        else
        {
            // Something bad happened.
            // TODO: something.
        }
        break;
    }
}

/* print_state
 *
 * Print the current connection state.
 */
static void print_state(context_t *ctx)
{
    if (ctx->connection_state == ESTABLISHED)
    {
        printf("state: ESTABLISHED \n");
    }
    else if (ctx->connection_state == LISTEN)
    {
        printf("state: LISTEN \n");
    }
    else if (ctx->connection_state == SYN_SENT)
    {
        printf("state: SYN_SENT\n");
    }
    else if (ctx->connection_state == SYN_RECEIVED)
    {
        printf("state: SYN_RECEIVED \n");
    }
    else if (ctx->connection_state == FIN_WAIT_1)
    {
        printf("state: FIN_WAIT_1\n");
    }
    else if (ctx->connection_state == FIN_WAIT_2)
    {
        printf("state: FIN_WAIT_2\n");
    }
    else if (ctx->connection_state == CLOSE_WAIT)
    {
        printf("state: CLOSE_WAIT\n");
    }
    else if (ctx->connection_state == CLOSING)
    {
        printf("state: CLOSING \n");
    }
    else if (ctx->connection_state == LAST_ACK)
    {
        printf("state: LAST_ACK \n");
    }
    else if (ctx->connection_state == TIME_WAIT)
    {
        printf("state: TIME_WAIT \n");
    }
    else if (ctx->connection_state == CLOSED)
    {
        printf("state: CLOSED \n");
    }
    else
    {
        printf("state: UNKNOWN =====\n");
    }
}

/* is_valid_ack
 *
 * Return TRUE if this is a valid ACK packet, FALSE otherwise.
 */
static bool_t is_valid_ack(context_t *ctx, STCPHeader *header)
{
    // Sanity checks.
    assert(ctx);
    assert(header);

    if ((ctx->send_space.snd_una < ntohl(header->th_ack)) & 
         (ntohl(header->th_ack) <= ctx->send_space.snd_nxt))
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

/* is_in_order
 *
 * Return TRUE is this is the next expected segment, FALSE otherwise.
 */
static bool_t is_in_order(context_t *ctx, STCPHeader *header)
{
    if (ntohl(header->th_seq) == ctx->receive_space.rcv_nxt)
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

/* process_receive
 *
 * Called from network_receive, procsses TCP packets recieved and performs the
 * the following steps:
 *
 * 1. TODO: Fill in
 *
 * Return TRUE on success, FALSE on failure or placed packet on out of order queue.
 */
static bool_t process_receive(mysocket_t sd, context_t *ctx, STCPHeader *header,
                    int len)
{
    bool_t success = TRUE;
    bool_t in_order_fin = FALSE;

    // If this is an ACK packet
    if (header->th_flags & TH_ACK)
    {
        // Check that it is a valid ACK
        if (is_valid_ack(ctx, header))
        {
            ctx->send_space.snd_una = ntohl(header->th_ack);
        }

        // Acquire lock.
        pthread_mutex_lock(&(ctx->retransmit_mutex));

        bool_t removed_segment = FALSE;

        // Search the retransmission list for this ACK.
        ctx->retransmit = dequeue_segment(ntohl(header->th_ack),
                                            ctx->retransmit, &removed_segment);

        // Check if this did not acknowledge anything new.
        if (removed_segment)
        {
            // Account for dropped ACK packets.
            ctx->receive_space.rcv_nxt = ntohl(header->th_seq) + 1;
            ctx->last_ack = ntohl(header->th_ack);
        }

        // Release lock.
        pthread_mutex_unlock(&(ctx->retransmit_mutex));
    }
    if (header->th_flags & TH_SYN)
    {
    }
    if (header->th_flags & TH_FIN)
    {

        // Make sure all data has been passed up to the application
        // Check if this is out of order.
        if (is_in_order(ctx , header))
        {
            // Do this after the data has been processed below.
            in_order_fin = TRUE;
        }
        else
        {
            // If out of order, place on the out of order queue.
            ctx->out_of_order = insert_out_of_order_segment(ctx, header, len);
            success = FALSE;
        }
    }

    char *data = TCP_START_DATA(header);

    // If segment has data, process it.
    int data_len = len - (sizeof(STCPHeader));

    if (data_len > 0)
    {

        // Discard the packet if we already recieved it's data.
        if ((ntohl(header->th_seq) + data_len) > ctx->receive_space.rcv_nxt)
        {
            /* If need be, trim data */

            // If the given data is past the left edge of the window.
            if (ntohl(header->th_seq) < ctx->receive_space.rcv_nxt)
            {
                // Discard the overlapping data.
                // TODO: Move data pointer to the right.
            }

            // If the segment is within the window.
            if (is_in_window(ctx, header, data_len))
            {
                // Check if this segment is the next expected one.
                if (is_in_order(ctx, header))
                {
                    // Check to see if the data is past the right edge of the window.
                    // TODO: data_len = MIN(ctx->receive_space.rcv_wnd, (data + data_len));

                    struct segment *prev = NULL;
                    struct segment *next = ctx->out_of_order;

                    // First check if there are any old out of sequence segments that
                    // this segment handles and remove them
                    while (next != NULL &&
                        (ntohl(next->packet->th_seq) + next->data_len) <=
                                (ntohl(header->th_seq) + data_len))
                    {
                        prev = next;
                        next = next->next;
                        free_segment(prev);
                    }

                    // TODO: Check if this segment overlaps with an out of sequence
                    // segment
                    ctx->out_of_order = next;

                    // Send the data to the application.
                    stcp_app_send(sd, data, data_len);

                    // Update send window: increase rcv.nxt.
                    ctx->receive_space.rcv_nxt += data_len;

                    // Send ACK
                    create_send_ack(sd, ctx, header);

                    // Check if this effects the out_of_order queue.
                    // (i.e., if any out of order segments are now in order)
                    success = success & check_out_of_order(sd, ctx);

                    // Else, see if this segment overlaps with any out_of_order
                    // segments.'
                    // TODO
                }
                // Else, this segment is out of order.
                else
                {
                    // Queue the segment on the out_of_order_queue.
                    ctx->out_of_order =
                        insert_out_of_order_segment(ctx, header, len);

                    success = FALSE;
                }
            }
            // The segment is not within the window, send empty ACK.
            else
            {
                // Send empty ACK.
                //create_send_ack(sd, ctx, header);
            }
        }
        else
        {
            // Else, this is a duplicate packet, so just drop it.
            // Send a empty ACK.
            //create_send_ack(sd, ctx, header);
        }
    }

    // If the data was passed up to the application successfully and this is a FIN.
    if (success & in_order_fin)
    {
        ctx->send_space.snd_una = ntohl(header->th_ack);
        ctx->receive_space.rcv_nxt++;

        // Tell the application no more data is arriving.
        stcp_fin_received(sd);

        // Send a empty ACK. The data (if it's there) will be sent
        // separately later on in this function.
        create_send_ack(sd, ctx, header);

        // Since this is a FIN, than anything that was receieved out of
        // order must have already been received in order, so discard the
        // out_of_order queue.
        struct segment *prev = NULL;

        while (ctx->out_of_order != NULL)
        {
            prev = ctx->out_of_order->next;
            free_segment(prev);
        }
    }

    return success;
}

/* is_in_window
 *
 * Return TRUE if the given data falls within the window, i.e.,
 * RCV.NXT =< SEG.SEQ < RCV.NXT+RCV.WND or RCV.NXT =< SEG.SEQ+SEG.LEN-1 < RCV.NXT+RCV.WND)
 */
static bool_t is_in_window(context_t *ctx, STCPHeader *header, int data_len)
{
    // Check the data falls within the window.
    if (((ctx->receive_space.rcv_nxt <= ntohl(header->th_seq)) &
         (ntohl(header->th_seq) < (ctx->receive_space.rcv_nxt + ctx->receive_space.rcv_wnd))) |
         ((ctx->receive_space.rcv_nxt <= (ntohl(header->th_seq + data_len-1))) &
          ((ntohl(header->th_seq + data_len-1) <
            (ctx->receive_space.rcv_nxt + ctx->receive_space.rcv_wnd)))))
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

/* process_close
 *
 *
 */
static int process_close(mysocket_t sd, context_t *ctx)
{
    // Send a FIN and wait until the retransmission queue is empty or
    // the connection is terminated due to a timeout.
    switch(ctx->connection_state)
    {
    case ESTABLISHED:
        // Send FIN
        if ((create_send_fin(sd, ctx)) <= 0)
        {
            // An error happened while sending, which we can't recover
            // from. So return.
            return -1;
        }
        else
        {
            // If successful, state = FIN_WAIT_1
            ctx->connection_state = FIN_WAIT_1;
        }
        break;
    case CLOSE_WAIT:
        // Send FIN
        if ((create_send_fin(sd, ctx)) <= 0)
        {
            // An error happened while sending, which we can't recover
            // from. So return.
            return -1;
        }
        else
        {
            // If successful, state = LAST_ACK
            ctx->connection_state = LAST_ACK;
        }
        break;
    default:
        // Do not need to do anything.
        break;
    }

    // Success.
    return 0;
}

/* create_send_fin
 *
 * Create a FIN, send it to the peer and add it to the retransmission queue.
 *
 * Returns the number of bytes sent.
 */
int create_send_fin(mysocket_t sd, context_t *ctx)
{
    // Sanity check.
    assert(ctx);

    ssize_t bytes_sent;

    STCPHeader *fin = create_tcp_packet(TH_FIN, ctx->send_space.snd_nxt,
                    ctx->receive_space.rcv_nxt, 0);

    bytes_sent =
            stcp_network_send(sd, fin, sizeof(STCPHeader), NULL);

    // Send failed.
    if (bytes_sent <= 0)
    {
        printf("* Sending FIN failed: Connection refused\n");
        errno = ECONNREFUSED;
    }
    else
    {
        // Increment snd_nxt.
        update_context_send(ctx, 1);

        // Place the packet on the retransmission queue.
        enqueue_retransmit_segment(ctx, fin, 0);
    }

    return bytes_sent;
}

/* create_send_ack
 *
 * Create and empty ACK and send it. We don't really care if the send fails.
 */
void create_send_ack(mysocket_t sd, context_t *ctx, STCPHeader *packet)
{
    // Sanity check.
    assert(ctx);

    ssize_t bytes_sent;

    STCPHeader *ack = create_tcp_packet(TH_ACK, ctx->send_space.snd_nxt,
                        ctx->receive_space.rcv_nxt, 0);

    bytes_sent = stcp_network_send(sd, ack, sizeof(STCPHeader), NULL);

    // Send failed, but don't return.
    if (bytes_sent <= 0)
    {
        printf("* Sending ACK failed: Connection refused\n");
        errno = ECONNREFUSED;
    }
    else
    {
        // Increment snd_nxt.
        update_context_send(ctx, 1);
        // Update the window.
        // TODO: Is this right?
        packet->th_win = ctx->send_space.snd_wnd;
    }

    free(ack);
}

/**********************************************************************/
/* our_dprintf
 *
 * Send a formatted message to stdout.
 * 
 * format               A printf-style format string.
 *
 * This function is equivalent to a printf, but may be
 * changed to log errors to a file if desired.
 *
 * Calls to this function are generated by the dprintf amd
 * dperror macros in transport.h
 */
void our_dprintf(const char *format,...)
{
    va_list argptr;
    char buffer[1024];

    assert(format);
    va_start(argptr, format);
    vsnprintf(buffer, sizeof(buffer), format, argptr);
    va_end(argptr);
    fputs(buffer, stdout);
    fflush(stdout);
}

/**************************** HELPER FUNCTIONS *******************************/
/* create_tcp_packet
 *
 * Return a pointer to a new STCPHeader with the given fields filled in.
 * If a packet is specified, copy it into the packet field.
 */
static STCPHeader *create_tcp_packet(uint8_t th_flags, tcp_seq th_seq,
                                        tcp_seq th_ack, int len, STCPHeader *packet)
{
    STCPHeader *header = (STCPHeader *) calloc(1, sizeof(STCPHeader) + len);

    // Sanity check.
    assert(header);

    // TODO: check for flags we handle?

    header->th_flags = th_flags;
    // Type tcp_seq is uint32_t, so we need to convert!
    header->th_seq = htonl(th_seq);
    header->th_ack = htonl(th_ack);
    // If there are no options in the header.
    header->th_off = 5;
    // Type tcp_seq is uint16_t, so we need to convert!
    header->th_win = htons(RECIEVER_WIN_SIZE);

    // Copy the data from the given packet.
    if (packet != NULL)
    {
        memcpy((char *) TCP_START_DATA(header),
               (char *) TCP_START_DATA(packet), len);
    }

    return header;
}

/* update_context_send
 *
 * Update the given context when sending data by performing the following:
 *    1. Advances snd_nxt
 */
static void update_context_send(context_t *ctx, int len)
{
   assert(ctx);

   ctx->send_space.snd_nxt+=len;
}

/* update_context_receive
 *
 * Update the given context when receiving data by performing the following:
 *    1. When receiving an ACK, advance snd_una
 *    2. When receiving an segment, advance rcv_nxt
 */
static void update_context_receive(context_t *ctx, bool is_ack)
{
   assert(ctx);

   if (is_ack)
   {
      ctx->send_space.snd_una++;
   }

   ctx->receive_space.rcv_nxt++;
}

/* update_send_wnd
 *
 * Update the size of the sender window which is the minimum of the other
 * side's advertised receiver window and the congestion window.
 */
static void update_send_wnd(context_t *ctx, uint16_t wnd)
{
    assert(ctx);

    ctx->send_space.snd_wnd = MIN(wnd, CONGESTION_WIN_SIZE);
}

/* get_timeout
 *
 * Return a struct timespec representing a system time for a timeout.
 */
static struct timeval *get_timeout()
{
    // This must be freed!
    struct timeval *tv = (struct timeval *)malloc(sizeof(struct timeval));
    assert(tv);

    gettimeofday(tv, NULL);

    tv->tv_sec += MAX_TIMEOUT_SEC;

    return tv;
}


/* print_data
 *
 * Print the data contained in the given buffer and the given length.
 */
static void print_data(char *buf, int len)
{
    assert(buf);

    int i;

    printf("DATA BUF: ");

    for (i = 0; i < len; i++)
    {
        printf("%c", buf[i]);
    }

    if (!(buf[len-1] == '\\' && buf[len] == 'n'))
        printf("\n");
}


/*
 * print_context
 *
 * Prints the context_t structure elements.
 */
static void print_context(context_t *ctx)
{
   printf("==================== CONTEXT ====================\n");
   printf("done: %d\n", ctx->done);
   print_state(ctx);
   printf("initial_sequence_number: %d\n", ctx->initial_sequence_num);
   printf("send unacknowledged: %d\n", ctx->send_space.snd_una);
   printf("send next: %d\n", ctx->send_space.snd_nxt);
   printf("send window: %d\n", ctx->send_space.snd_wnd);
   printf("receive next: %d\n", ctx->receive_space.rcv_nxt);
   printf("receive window: %d\n", ctx->receive_space.rcv_wnd);
   printf("last ack: %d\n", ctx->last_ack);

   printf("=================================================\n");
}

/* print_queue
 *
 */
static void print_queue(struct segment *queue)
{
    struct segment *head = queue;

    while (head != NULL)
    {
        printf("seq: %d, ack: %d, len: %d, # retransmissions: %d | ",
            ntohl(head->packet->th_seq), ntohl(head->packet->th_ack),
            head->data_len, head->num_retransmission);

        head = head->next;
    }

    printf("NULL");

    printf("\n");
}

/* print_out_of_order
 *
 */
static void print_out_of_order(context_t *ctx)
{
    assert(ctx);

    struct segment *head = ctx->out_of_order;

    printf("* *     OUT OR ORDER: ");
    while (head != NULL)
    {
        printf("seq: %d, ack: %d | ",
            ntohl(head->packet->th_seq), ntohl(head->packet->th_ack));

        head = head->next;
    }

    printf("NULL");

    printf("\n");
}

/* check_out_of_order
 *
 * Checks to see if any of the out of order segments are not in order, if so
 * processes the segment.
 * TODO: Handle trimming
 * Returns TRUE if the packet was processes correctly or if no out of order
 * segments were affected.
 */
static bool_t check_out_of_order(mysocket_t sd, context_t *ctx)
{
    // Sanity check.
    assert(ctx);

    struct segment *prev = NULL;
    struct segment *next = ctx->out_of_order;
    bool_t success = TRUE;

    while (next != NULL && ntohl(next->packet->th_seq) ==
          ctx->receive_space.rcv_nxt)
    {
        prev = next;

        ctx->out_of_order = next->next;

        success = success & process_receive(sd, ctx, next->packet,
                    next->data_len);

        next = next->next;

        free_segment(prev);
    }

    return success;
}

/* create_segment
 *
 * Return a pointer to a new segment with the given fields filled in.
 */
static struct segment *create_segment(struct segment *next, int data_len,
                                     STCPHeader *packet,
                                     struct timeval *timeout, int num_rt)
{
    // This must be freed when the ACK is receieved!
    struct segment *seg = (struct segment*)calloc(1, sizeof(struct segment));
    assert(seg);

    seg->next = next;
    seg->data_len = data_len;
    seg->packet = packet;
    seg->timeout = timeout;
    seg->num_retransmission = num_rt;

    return seg;
}

/* enqueue_retransmit_segment
 *
 * Add the given packet to the tail of the retransmission queue.
 *
 * The copied packet is freed later by dequeue_segment().
 */
static void enqueue_retransmit_segment(context_t *ctx, STCPHeader *packet,
                                       int len)
{
    assert(ctx && packet);

    // Acquire lock.
    pthread_mutex_lock(&(ctx->retransmit_mutex));

    // Create a segment to be placed at the end of the retransmit queue. This 
    // must be freed when it is dequeued!
    struct segment *seg = create_segment(NULL, len, packet, get_timeout(), 0);
    struct segment *head = ctx->retransmit;

    if (head != NULL)
    {
        while (head->next != NULL)
        {
            head = head->next;
        }

        head->next = seg;
    }
    else
    {
        ctx->retransmit = seg;
    }

    // Release lock.
    pthread_mutex_unlock(&(ctx->retransmit_mutex));
}

/* dequeue_segment
 *
 * Searches the given queue for segment(s) with a sequence number <= the given
 * acknowledgement number, those that are found are moved because they are not
 * acknowledged and so do not need to be retransmitted.
 *
 * Returns a pointer to the head of the list.
 */

static struct segment *dequeue_segment(tcp_seq th_ack, struct segment *queue,
                                        bool_t *removed)
{
    struct segment *cur, *head, *prev, *temp;

    prev = NULL;
    cur = queue;
    // Save a pointer to the head.
    head = queue;

    // Find a segment to delete.
    while (cur != NULL)
    {
        // If this is a segment that can be removed.
        if (ntohl(cur->packet->th_seq) < th_ack)
        {
            // Check if this is the head of the list.
            if (prev == NULL)
            {
                // Then set the head to be the next segment.
                head = cur->next;
            }
            else
            {
                prev->next = cur->next;
            }

            temp = cur;
            cur = cur->next;
            *removed = TRUE;
            free_segment(temp);
        }
        else
        {
            prev = cur;
            cur = cur->next;
        }
    }

    return head;
}

/* free_segment
 *
 * Free the resources associated with the given segment.
 */
static void free_segment(struct segment *segment)
{
    free(segment->timeout);
    free(segment->packet);
}

/* segment_fill_packet
 *
 * Create a new STCP packet, filling in the fields with the supplied packet's
 * data. This must be freed when the segment is dequeued!.
 */
static void segment_fill_packet(struct segment *segment, STCPHeader *packet,
                                int len)
{
    segment->packet =
            create_tcp_packet(packet->th_flags, htonl(packet->th_seq),
                                 htonl(packet->th_ack), len, packet);
    segment->packet->th_win = packet->th_win;
}
/* insert_out_of_order_segment
 *
 * Creates a segment from the given packet and inserts it into the out of order
 * queue in the correct position (i.e., orders it based on sequence number).
 * TODO: Trim segments after insert.
 *
 * Returns a pointer to the header of the out_of_order queue.
 */
static struct segment *insert_out_of_order_segment(context_t *ctx,
                                        STCPHeader *packet, int len)
{
    struct segment *head = ctx->out_of_order;

    // First create a new segment.
    // This must be freed when it is dequeued!
    struct segment *segment = create_segment(NULL, len, NULL,
                                             get_timeout(), 0);

    // If the out of order queue is empty, this segment is the head.
    if (ctx->out_of_order == NULL)
    {
        // Copy the packet into the segment's packet buffer.
        // This packet must be freed when the segment is dequeued!
        segment_fill_packet(segment, packet, len);
        head = segment;
    }
    // Else, insert this segment into the correct position on the queue.
    // If there exists a segment with the same sequence number, keep the
    // one with more data.
    else
    {
        struct segment *prev = NULL;
        struct segment *next;

        for (next = head; next != NULL; next = next->next)
        {
            // If there already exists an out of order segment with the same
            // sequence number.
            if (ntohl(packet->th_seq) == ntohl(next->packet->th_seq))
            {
                if (next->data_len < segment->data_len)
                {
                    segment->next = next->next;

                    // Replace this segment and maybe more with the new
                    // segment.
                    if (prev == NULL)
                    {
                        head = segment;
                    }
                    else
                    {
                        prev->next = segment;
                    }

                    // TODO: Check if we need to trim segments.
                    // Copy the packet into the segment's packet buffer.
                    // This packet must be freed when the segment is dequeued!
                    segment_fill_packet(segment, packet, len);
                }
                // We do not need the new segment, so free it.
                else
                {
                    free(segment);
                    break;
                }
            }
            // If this segment belongs behind of the current segment.
            else if (ntohl(packet->th_seq) < ntohl(next->packet->th_seq))
            {
                segment->next = next;

                // Check if it needs to be inserted as the head.
                if (prev == NULL)
                {
                    head = segment;
                }
                else
                {
                    prev->next = segment;
                }

                // Copy the packet into the segment's packet buffer.
                // This packet must be freed when the segment is dequeued!
                segment_fill_packet(segment, packet, len);

                break;
            }
            // Otherwise, the segment should be the tail.
            else if (next->next == NULL &&
                    ntohl(packet->th_seq) > ntohl(next->packet->th_seq))
            {
                next->next = segment;
                // Copy the packet into the segment's packet buffer.
                // This packet must be freed when the segment is dequeued!
                segment_fill_packet(segment, packet, len);
                break;
            }

            prev = next;
        }
    }

    return head;
}

/* get_tail
 *
 * Return a pointer to the tail of the given queue, NULL if the queue is NULL.
 */
static struct segment *get_tail(struct segment *queue)
{
    struct segment *tail = queue;

    if (queue != NULL)
    {
        while (queue->next != NULL)
        {
            tail = queue->next;
        }
    }

    return tail;
}
